From 4952c9cd87cf56e2de1087c58e58410617b7fd90 Mon Sep 17 00:00:00 2001
From: tsteven4 <tsteven4@users.noreply.github.com>
Date: Sat, 30 Dec 2017 16:21:12 -0700
Subject: [PATCH] fixes for the position and track filters. (#157)

the position filter could corrupt the route_waypt_ct for routes/tracks.
the track filter underestimated speed when there were multiple
waypoints with the same time.
---
 position.cc    | 22 +++++++++++++++++-----
 trackfilter.cc | 43 +++++++++++++++++++++++++++++++------------
 2 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/position.cc b/position.cc
index 38ce7c0a2..0d5d54725 100644
--- a/position.cc
+++ b/position.cc
@@ -87,7 +87,7 @@ position_runqueue(queue* q, int nelems, int qtype)
   qlist = (int*) xcalloc(nelems, sizeof(*qlist));
 
 #if NEWQ
-  foreach(Waypoint* waypointp, waypt_list) {
+  foreach (Waypoint* waypointp, waypt_list) {
     comp[i] = waypointp;
 #else
   QUEUE_FOR_EACH(q, elem, tmp) {
@@ -166,18 +166,30 @@ position_runqueue(queue* q, int nelems, int qtype)
 }
 
 static void
-position_process_route(const route_head* rh)
+position_process_any_route(const route_head* rh, int type)
 {
   int i = rh->rte_waypt_ct;
 
   if (i) {
     cur_rte = (route_head*)rh;
-    position_runqueue((queue*)&rh->waypoint_list, i, rtedata);
+    position_runqueue((queue*)&rh->waypoint_list, i, type);
     cur_rte = NULL;
   }
 
 }
 
+static void
+position_process_rte(const route_head* rh)
+{
+  position_process_any_route(rh, rtedata);
+}
+
+static void
+position_process_trk(const route_head* rh)
+{
+  position_process_any_route(rh, trkdata);
+}
+
 static void
 position_noop_w(const Waypoint*)
 {
@@ -196,8 +208,8 @@ void position_process()
     position_runqueue(&waypt_head, i, wptdata);
   }
 
-  route_disp_all(position_process_route, position_noop_t, position_noop_w);
-  track_disp_all(position_process_route, position_noop_t, position_noop_w);
+  route_disp_all(position_process_rte, position_noop_t, position_noop_w);
+  track_disp_all(position_process_trk, position_noop_t, position_noop_w);
 }
 
 void
diff --git a/trackfilter.cc b/trackfilter.cc
index 601ae0ab8..40ff8f302 100644
--- a/trackfilter.cc
+++ b/trackfilter.cc
@@ -817,10 +817,12 @@ trackfilter_synth()
   queue* elem, *tmp;
   Waypoint* wpt;
 
-  double oldlat = -999;
-  double oldlon = -999;
-  time_t oldtime = 0;
-  int first = 1;
+  double last_course_lat;
+  double last_course_lon;
+  double last_speed_lat;
+  double last_speed_lon;
+  time_t last_speed_time;
+  int first;
   fix_type fix;
   int nsats = 0;
 
@@ -839,33 +841,50 @@ trackfilter_synth()
       }
       if (first) {
         if (opt_course) {
+          // TODO: the course value 0 isn't valid, wouldn't it be better to UNSET course?
           WAYPT_SET(wpt, course, 0);
         }
         if (opt_speed) {
+          // TODO: the speed value 0 isn't valid, wouldn't it be better to UNSET speed?
           WAYPT_SET(wpt, speed, 0);
         }
         first = 0;
+        last_course_lat = wpt->latitude;
+        last_course_lon = wpt->longitude;
+        last_speed_lat = wpt->latitude;
+        last_speed_lon = wpt->longitude;
+        last_speed_time = wpt->GetCreationTime().toTime_t();
       } else {
         if (opt_course) {
-          WAYPT_SET(wpt, course, heading_true_degrees(RAD(oldlat),
-                    RAD(oldlon),RAD(wpt->latitude),
+          WAYPT_SET(wpt, course, heading_true_degrees(RAD(last_course_lat),
+                    RAD(last_course_lon),RAD(wpt->latitude),
                     RAD(wpt->longitude)));
+          last_course_lat = wpt->latitude;
+          last_course_lon = wpt->longitude;
         }
         if (opt_speed) {
-          if (oldtime != wpt->GetCreationTime().toTime_t()) {
+          if (last_speed_time != wpt->GetCreationTime().toTime_t()) {
+            // If we have mutliple points with the same time and
+            // we use the pair of points about which the time ticks then we will
+            // underestimate the distance and compute low speeds on average.
+            // Therefore, if we have multiple points with the same time use the
+            // first ones with the new times to compute speed.
+            // Note that points with the same time can occur because the input
+            // has truncated times, or because we are truncating times with
+            // toTime_t().
             WAYPT_SET(wpt, speed, radtometers(gcdist(
-                                                RAD(oldlat), RAD(oldlon),
+                                                RAD(last_speed_lat), RAD(last_speed_lon),
                                                 RAD(wpt->latitude),
                                                 RAD(wpt->longitude))) /
-                      labs(wpt->GetCreationTime().toTime_t()-oldtime));
+                      labs(wpt->GetCreationTime().toTime_t()-last_speed_time));
+            last_speed_lat = wpt->latitude;
+            last_speed_lon = wpt->longitude;
+            last_speed_time = wpt->GetCreationTime().toTime_t();
           } else {
             WAYPT_UNSET(wpt, speed);
           }
         }
       }
-      oldlat = wpt->latitude;
-      oldlon = wpt->longitude;
-      oldtime = wpt->GetCreationTime().toTime_t();
     }
   }
 }
-- 
2.30.2